#include <rov/motor.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>


#define DBG		1
#define BUFFER_SIZE	8
#define PORT		32000
#define BACKLOG		1


int main(int argc, char ** argv)
{
    int listenfd, connfd, n;
    struct sockaddr_in servaddr, cliaddr;
    socklen_t clilen;
    char buf[BUFFER_SIZE];

    int motorStatus = initMotors();

#if DBG
  sleep(1);
  motorOutput();
#endif

    printf("motor status: %d\n", motorStatus);

    listenfd = socket(AF_INET, SOCK_STREAM, 0);

    bzero(&servaddr, sizeof(servaddr));

    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(PORT);

    bind(listenfd, (struct sockaddr *) &servaddr, sizeof(servaddr));

    listen(listenfd, BACKLOG);

    for(;;)
    {
        clilen = sizeof(cliaddr);
        connfd = accept(listenfd, (struct sockaddr *) &cliaddr, &clilen);

printf("new client\n");

       for(;;)
       {
           n = recvfrom(connfd, buf, BUFFER_SIZE, 0, (struct sockaddr *) &cliaddr, &clilen);

           if (n == 0)
           {
               printf("connection closed by client\n");
               break;
           } else if (n < 0)
           {
               printf("error: connection closed\n");
               break;
           }

           buf[n] = 0;

//printf("received: %s\n", buf);

           int motorIndex = ntohl(* (int *) buf);
           int motorSpeed = ntohl(* (int *) &buf[4]);

//printf("setting motor %d to %d\n", motorIndex, motorSpeed);

setMotorSpeed(motorIndex, motorSpeed);

n = 5;
buf[0] = 'k';
buf[1] = 't';
buf[2] = 'h';
buf[3] = 'x';
buf[4] = 0;

           sendto(connfd, buf, n, 0, (struct sockaddr *) &cliaddr, sizeof(cliaddr));

#if DBG
  sleep(1);
  motorOutput();
#endif

       }

       close(connfd);
    }

    close(listenfd);

    return 0;
}
